
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Float32MultiArray.h"
#include "navigation_msg/odom.h"
#include "std_msgs/Int8.h"//ROS自定义的数据类型String所在的头文件
#include <vector>
#include <queue>

using namespace std;

namespace move_base {

class Small_car
{
   
    public:

      Small_car();
      ~Small_car();

      void stop_car();
      void turn_right();
      void turn_left();
      void turn_stop();
      void go_forward();
      void go_backward();
      void run();
      

      void set_car_state(uint16_t movestate);
      uint16_t get_car_state();
      void set_car_contactflag(uint16_t contactflag);
      uint16_t get_car_contactflag();
      void set_car_speed(uint16_t speed);
      uint16_t get_car_speed();
      void slam_map_display();
      void send_control_command(int16_t speed_enable,int16_t target_speed,int16_t dir_enable,int16_t target_angle,int16_t  target_dir);
      void follow_given_target(float given_angle);
      void doubleCallback(const std_msgs::Float64MultiArrayConstPtr& vec_double);
      void SlamORBCallback(const std_msgs::Float32MultiArrayConstPtr& vec_double);
      void ladarCallback(const std_msgs::Int8 ladar_diatance);
      void ImuDataCallback(const std_msgs::Float64MultiArrayConstPtr& imu_message);

   private:
    
      uint16_t move_state;
      uint16_t contact_flag;
      uint16_t move_speed;
      uint16_t control_flag;
      uint16_t right_wheel_speed;
      uint16_t left_wheel_speed;
      uint16_t motor_wheel_speed;
      uint16_t disc_turn_angle;
      uint16_t disc_turn_pulse;
      int turn_enable;
      int turn_right_enable;
      int turn_left_enable;


      float imu_x;
      float imu_y;
      float imu_angle;

      float temp_slam_x;
      float temp_slam_y;
      float temp_slam_z;

      float target_slam_x;
      float target_slam_y;
      float target_slam_z;

      float slam_angle;

      float gps_x;
      float gps_y;
      float gps_angle;

      float slam_distance;

      float slam_given_angle;
      ros::NodeHandle motion_handle;
      ros::Publisher Control_Command ;
      ros::Publisher pub_slam_data;
      int8_t obstack_inqure;
      bool obstack_appear;
      queue<int8_t> obstack_dis;
  };
}
